#include "rclcpp/rclcpp.hpp"
#include <rclcpp_action/rclcpp_action.hpp>
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_array.hpp" // 新增PoseArray支持
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/Constants.hpp>
#include <string>
#include <thread>
#include <chrono>
#include <mutex>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <nav2_msgs/action/follow_waypoints.hpp> // 新增FollowWaypoints支持
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <unordered_map>
#include <regex>
#include <memory>
#include <unistd.h> // getpid()
#include <sched.h>  // CPU_SET/sched_setaffinity
#include <cstring>  // strcmp
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "std_msgs/msg/bool.hpp"
#include <cmath>
#include <atomic>
#include <future>
class moveToGpsTarget : public rclcpp::Node
{
public:
    moveToGpsTarget()
        : Node("move_to_gps_target_node"),
          apmControllerNameSpace_("/apm_drone"),
          navigation_succeeded_(false), // 初始化为 false
          tf_buffer_(this->get_clock()),
          tf_listener_(tf_buffer_),
          bt_enabled_(false),
          bt_thread_running_(false),
          single_gps_nav2_(false)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // 等待2秒
        // 检测时钟源
        auto topics = this->get_topic_names_and_types();
        for (const auto &[name, types] : topics)
        {
            if (name == "/clock")
            {
                use_sim_time_ = true;
                break;
            }
        }
        this->set_parameter(rclcpp::Parameter("use_sim_time", use_sim_time_));
        // 订阅 'current_mode' 话题
        subscription_mode_ = this->create_subscription<std_msgs::msg::String>(
            apmControllerNameSpace_ + "/current_mode", 10,
            std::bind(&moveToGpsTarget::current_mode_callback, this, std::placeholders::_1));
        // 发布到 'target_mode' 话题
        publisher_target_mode_ = this->create_publisher<std_msgs::msg::String>(
            apmControllerNameSpace_ + "/target_mode", 10);
        // 订阅控制使能信号
        subscription_bt_enable_ = this->create_subscription<std_msgs::msg::Bool>(
            apmControllerNameSpace_ + "/bt_enable", 10,
            std::bind(&moveToGpsTarget::bt_enable_callback, this, std::placeholders::_1));

        // 新增多目标点订阅
        subscription_mult_pose_ = this->create_subscription<geometry_msgs::msg::PoseArray>(
            "/mult_pose", 10,
            std::bind(&moveToGpsTarget::mult_pose_callback, this, std::placeholders::_1));

        // 初始化FollowWaypoints客户端
        this->action_client_follow_ = rclcpp_action::create_client<nav2_msgs::action::FollowWaypoints>(
            this, "follow_waypoints");
        // 发布目标位置
        publisher_goal_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
            "/goal_pose", 10);
        publisher_target_cmd_vel_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/cmd_vel", 10);
        publisher_enable_apriltag_ = this->create_publisher<std_msgs::msg::Bool>(
            apmControllerNameSpace_ + "/enable_apriltag_processor", 10);
        // 设置定时器更新所有tag信息
        update_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            [this]()
            { this->update_all_tags(); });
        // 创建一个 NavigateToPose 的客户端
        this->action_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this, "navigate_to_pose");
    }
    // 获取当前存储的所有tag信息（线程安全）
    std::unordered_map<int, geometry_msgs::msg::TransformStamped> get_tags()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        return tags_;
    }
    ~moveToGpsTarget()
    {
        stop_bt_thread();
    }

private:
    void bt_enable_callback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data)
        {
            if (!bt_enabled_.exchange(true))
            {
                start_bt_thread();
            }
        }
        else
        {
            if (bt_enabled_.exchange(false))
            {
                stop_bt_thread();
                publish_target_mode("LAND");
            }
        }
    }

    void start_bt_thread()
    {
        std::lock_guard<std::mutex> lock(thread_mutex_);
        if (!bt_thread_running_)
        {
            bt_thread_running_ = true;
            move_to_gps_target_thread_ = std::thread(&moveToGpsTarget::move_to_gps_target, this);
            RCLCPP_INFO(this->get_logger(), "任务线程启动");
        }
    }

    void stop_bt_thread()
    {
        bt_thread_running_ = false;
        std::lock_guard<std::mutex> lock(thread_mutex_);
        if (move_to_gps_target_thread_.joinable())
        {
            move_to_gps_target_thread_.join();
        }
        // 取消所有导航目标
        if (action_client_)
        {
            auto cancel_result = action_client_->async_cancel_all_goals();
            RCLCPP_INFO(this->get_logger(), "取消所有导航目标");
        }
    }

    // 订阅回调函数，更新当前模式
    void current_mode_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        current_mode_ = msg->data; // 更新当前模式
        // RCLCPP_INFO(this->get_logger(), "当前模式: '%s'", current_mode_.c_str());
    }
    void nav2_send_single_goal()
    {
        try
        {
            // 获取最新的 map 到 target 的变换
            auto transform = tf_buffer_.lookupTransform(
                "map", "target",
                tf2::TimePointZero,
                tf2::durationFromSec(1.0) // 等待1秒超时
            );

            // 创建导航目标消息
            auto goal_msg = nav2_msgs::action::NavigateToPose::Goal();
            goal_msg.pose.header.stamp = this->get_clock()->now();
            goal_msg.pose.header.frame_id = "map";

            // 设置位置
            goal_msg.pose.pose.position.x = transform.transform.translation.x;
            goal_msg.pose.pose.position.y = transform.transform.translation.y;
            goal_msg.pose.pose.position.z = 0.0;

            // 设置方向（直接使用TF的旋转）
            goal_msg.pose.pose.orientation = transform.transform.rotation;

            // 创建发送选项
            auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
            send_goal_options.result_callback = std::bind(&moveToGpsTarget::result_callback, this, std::placeholders::_1);

            // 发送目标
            RCLCPP_INFO(this->get_logger(),
                        "发送导航目标到 target 坐标系 [x: %.2f, y: %.2f, z: %.2f]",
                        goal_msg.pose.pose.position.x,
                        goal_msg.pose.pose.position.y,
                        goal_msg.pose.pose.position.z);

            this->action_client_->async_send_goal(goal_msg, send_goal_options);
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_ERROR(this->get_logger(),
                         "无法获取 map 到 target 的变换: %s", ex.what());
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(),
                         "发送目标时发生异常: %s", e.what());
        }
    }

    void mult_pose_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        mult_goals_.clear();

        for (const auto &pose : msg->poses)
        {
            geometry_msgs::msg::PoseStamped pose_stamped;
            pose_stamped.header = msg->header;
            pose_stamped.pose = pose;
            mult_goals_.push_back(pose_stamped);
        }

        // RCLCPP_INFO(this->get_logger(), "Received %zu waypoints", mult_goals_.size());
    }

    void nav2_send_mult_goal()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (mult_goals_.empty())
        {
            RCLCPP_ERROR(this->get_logger(), "No waypoints available");
            return;
        }

        auto goal_msg = nav2_msgs::action::FollowWaypoints::Goal();
        goal_msg.poses = mult_goals_;

        auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SendGoalOptions();
        send_goal_options.result_callback =
            [this](const rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>::WrappedResult &result)
        {
            switch (result.code)
            {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "Waypoints navigation succeeded");
                navigation_succeeded_ = true;
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_WARN(this->get_logger(), "Waypoints navigation canceled");
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "Waypoints navigation aborted");
                break;
            default:
                RCLCPP_ERROR(this->get_logger(), "Unknown waypoints result code");
                break;
            }
        };

        // 发送目标
        RCLCPP_INFO(this->get_logger(), "Sending %zu waypoints", mult_goals_.size());
        this->action_client_follow_->async_send_goal(goal_msg, send_goal_options);
    }
    void nav2_send_mult_goal_2()
    {
        // 复制目标点列表到本地变量（线程安全）
        std::vector<geometry_msgs::msg::PoseStamped> local_goals;
        {
            std::lock_guard<std::mutex> lock(mutex_);
            if (mult_goals_.empty())
            {
                RCLCPP_ERROR(this->get_logger(), "No waypoints available");
                return;
            }
            local_goals = mult_goals_;
        }

        const size_t total_goals = local_goals.size();
        size_t current_goal_idx = 0;
        navigation_succeeded_ = false; // 重置导航成功标志

        rclcpp::Rate loop_rate(4); // 4Hz = 250ms 循环周期

        // 处理前 N-1 个航点（通过持续发布到 /goal_pose）
        while (rclcpp::ok() &&
               bt_thread_running_ &&
               current_goal_idx < total_goals - 1)
        {
            // 发布当前目标点到导航系统
            const auto &current_goal = local_goals[current_goal_idx];
            publisher_goal_->publish(current_goal);

            try
            {
                // 获取无人机当前位置
                auto transform = tf_buffer_.lookupTransform(
                    "map", "base_link",
                    tf2::TimePointZero,
                    tf2::durationFromSec(0.5)); // 0.5秒超时

                // 计算水平距离
                const double dx = current_goal.pose.position.x -
                                  transform.transform.translation.x;
                const double dy = current_goal.pose.position.y -
                                  transform.transform.translation.y;
                const double distance = std::hypot(dx, dy);

                // 距离判断（1米阈值）
                if (distance < 1.0)
                {
                    RCLCPP_INFO(this->get_logger(),
                                "Waypoint %zu reached (%.2fm), proceeding to next",
                                current_goal_idx, distance);
                    current_goal_idx++;
                }
            }
            catch (const tf2::TransformException &ex)
            {
                RCLCPP_WARN(this->get_logger(),
                            "TF lookup failed: %s (retrying)", ex.what());
            }

            loop_rate.sleep();
        }

        // 使用动作客户端处理最后一个航点
        if (current_goal_idx == total_goals - 1)
        {
            auto goal_msg = nav2_msgs::action::NavigateToPose::Goal();
            goal_msg.pose = local_goals.back();

            // 配置动作客户端
            auto send_goal_options =
                rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
            send_goal_options.result_callback =
                std::bind(&moveToGpsTarget::result_callback, this, std::placeholders::_1);

            // 发送最终目标
            try
            {
                auto future_goal_handle =
                    action_client_->async_send_goal(goal_msg, send_goal_options);

                RCLCPP_INFO(this->get_logger(),
                            "Final waypoint sent via NavigateToPose (ID: %zu)",
                            current_goal_idx);
            }
            catch (const std::exception &e)
            {
                RCLCPP_ERROR(this->get_logger(),
                             "Failed to send final goal: %s", e.what());
            }
        }
    }
    void result_callback(const rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult &result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "Navigation succeeded.");
            navigation_succeeded_ = true; // 设置导航成功标志
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_WARN(this->get_logger(), "Navigation was canceled.");
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Navigation failed.");
            break;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code.");
            break;
        }
    }
    void update_all_tags()
    {
        std::vector<std::string> frames;
        try
        {
            frames = tf_buffer_.getAllFrameNames();
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN(
                this->get_logger(),
                "Failed to get frame list: %s",
                ex.what());
            return;
        }
        const std::string camera_frame = "camera_link";
        std::regex tag_pattern("tag_(\\d+)");
        const rclcpp::Time current_time = this->now();
        std::unordered_map<int, geometry_msgs::msg::TransformStamped> new_tags;
        for (const auto &frame : frames)
        {
            std::smatch match;
            if (!std::regex_match(frame, match, tag_pattern))
            {
                continue;
            }
            int tag_id;
            try
            {
                tag_id = std::stoi(match[1]);
            }
            catch (const std::exception &e)
            {
                RCLCPP_INFO(
                    this->get_logger(),
                    "Invalid tag ID format: %s",
                    frame.c_str());
                continue;
            }
            try
            {
                // Step 1: 获取camera_link到tag的原始变换
                auto camera_to_tag = tf_buffer_.lookupTransform(
                    camera_frame,
                    frame,
                    tf2::TimePoint());
                // Step 2: 时间有效性验证（使用camera_to_tag的时间戳）
                const double time_diff =
                    (current_time - rclcpp::Time(camera_to_tag.header.stamp)).seconds();
                if (time_diff > 10.0 || time_diff < 0.0)
                {
                    RCLCPP_INFO(
                        this->get_logger(),
                        "过期标签[tag_%d] 时间差: %.1fs",
                        tag_id, time_diff);
                    continue;
                }
                // Step 3: 转换到base_link坐标系
                geometry_msgs::msg::TransformStamped base_to_tag;
                try
                {
                    tf2::doTransform(camera_to_tag, base_to_tag, base_to_camera_transform_);
                    base_to_tag.header.frame_id = "base_link"; // 修正坐标系标识
                    base_to_tag.child_frame_id = frame;
                }
                catch (const tf2::TransformException &ex)
                {
                    RCLCPP_WARN(
                        this->get_logger(),
                        "坐标系转换失败[tag%d]: %s",
                        tag_id, ex.what());
                    continue;
                }
                // 保存有效标签
                new_tags[tag_id] = base_to_tag;
                RCLCPP_INFO(
                    this->get_logger(),
                    "有效标签[%d] 位置: [%.2f, %.2f, %.2f]",
                    tag_id,
                    base_to_tag.transform.translation.x,
                    base_to_tag.transform.translation.y,
                    base_to_tag.transform.translation.z);
            }
            catch (const tf2::TransformException &ex)
            {
                RCLCPP_INFO(
                    this->get_logger(),
                    "忽略不可用标签[%d]: %s",
                    tag_id, ex.what());
            }
        }
        // 合并有效数据
        {
            std::lock_guard<std::mutex> lock(mutex_);
            tags_.swap(new_tags);
        }
        // RCLCPP_INFO(
        //     this->get_logger(),
        //     "当前有效标签数量: %zu",
        //     tags_.size()
        // );
    }
    bool landing_procedure()
    {
        auto current_tags = get_tags();
        if (current_tags.empty())
        {
            RCLCPP_WARN(this->get_logger(), "未检测到可用降落标签");
            return false;
        }
        // 寻找最近标签
        int closest_id = -1;
        double min_distance = std::numeric_limits<double>::max();
        geometry_msgs::msg::TransformStamped closest_transform;
        for (const auto &[id, transform] : current_tags)
        {
            const auto &t = transform.transform.translation;
            double distance = std::hypot(t.x, t.y, t.z); // 三维欧氏距离
            if (distance < min_distance)
            {
                min_distance = distance;
                closest_id = id;
                closest_transform = transform;
            }
        }
        if (closest_id == -1)
        {
            RCLCPP_ERROR(this->get_logger(), "最近标签选择失败");
            return false;
        }
        RCLCPP_INFO(
            this->get_logger(),
            "选择最近标签[ID:%d] 距离: %.2fm 坐标: [%.2f, %.2f, %.2f]",
            closest_id, min_distance,
            closest_transform.transform.translation.x,
            closest_transform.transform.translation.y,
            closest_transform.transform.translation.z);
        // 控制逻辑
        const auto &t = closest_transform.transform.translation;
        geometry_msgs::msg::Twist cmd_vel_msg;
        // 水平控制
        double horizontal_distance = std::hypot(t.x, t.y);
        if (horizontal_distance > 5.0)
        { // 水平接近阶段
            cmd_vel_msg.linear.x = 0.8 * (t.x / horizontal_distance);
            cmd_vel_msg.linear.y = 0.8 * (t.y / horizontal_distance);
        }
        else
        {
            cmd_vel_msg.linear.x = 0.4 * t.x;
            cmd_vel_msg.linear.y = 0.4 * t.y;
        }
        // 高度控制（假设z轴正方向向上）
        const double current_height = std::abs(t.z);
        if (current_height > 20.0)
        {                                // 高空下降阶段
            cmd_vel_msg.linear.z = -1.0; // m/s
        }
        else if (current_height > 10.0)
        { // 低空精确调整
            cmd_vel_msg.linear.z = -0.5;
        }
        else if (current_height > 4.0)
        { // 触地阶段
            cmd_vel_msg.linear.z = -0.2;
        }
        else
        {
            cmd_vel_msg.linear.z = 0.0;
        }
        // 发布速度指令
        publisher_target_cmd_vel_->publish(cmd_vel_msg);
        // 降落条件判断
        if (horizontal_distance < 0.05 && current_height < 6.0)
        {
            RCLCPP_INFO(this->get_logger(), "满足降落条件，发送LAND指令");
            publish_target_mode("LAND");
            return true;
        }
        return false;
    }
    bool wait_tf()
    {
        try
        {
            base_to_camera_transform_ = tf_buffer_.lookupTransform(
                "base_link", "camera_link", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(), "成功获取base_link到camera_link的静态变换");
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN(this->get_logger(), "等待base_link到camera_link的静态变换: %s", ex.what());
            return false;
        }
        try
        {
            base_to_target_transform_ = tf_buffer_.lookupTransform(
                "map", "target", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(), "成功获取map到target的变换");
            single_gps_nav2_ = true;
        }
        catch (const tf2::TransformException &ex1)
        {
            try
            {
                base_to_target_transform_ = tf_buffer_.lookupTransform(
                    "map", "target_0", tf2::TimePointZero);
                RCLCPP_INFO(this->get_logger(), "成功获取map到targets的变换");
                return true;
            }
            catch (const tf2::TransformException &ex2)
            {
                RCLCPP_WARN(this->get_logger(), "等待map到target的变换: %s", ex1.what());
                RCLCPP_WARN(this->get_logger(), "等待map到targets的变换: %s", ex2.what());
                return false;
            }
            return false;
        }
        return true;
    }
    void publish_target_mode(std::string target_mode)
    {
        if (target_mode.empty())
            return;
        auto message = std_msgs::msg::String();
        message.data = target_mode;               // 设置目标模式为 "ARM"
        publisher_target_mode_->publish(message); // 发布到 'target_mode' 话题
        RCLCPP_INFO(this->get_logger(), "发布目标模式: '%s'", target_mode.c_str());
    }
    bool take_off_procedure()
    {
        try
        {
            while (rclcpp::ok() && bt_thread_running_)
            {
                {
                    std::lock_guard<std::mutex> lock(mutex_);
                    if (current_mode_ == "GUIDED")
                    {
                        RCLCPP_INFO(this->get_logger(), "无人机切换到GUIDED模式");
                        break;
                    }
                }
                publish_target_mode("GUIDED");
                std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 每1秒检查一次
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 等待2秒
            while (rclcpp::ok() && bt_thread_running_)
            {
                {
                    std::lock_guard<std::mutex> lock(mutex_);
                    if (current_mode_ == "ARM")
                    {
                        RCLCPP_INFO(this->get_logger(), "无人机切换到ARM模式");
                        break;
                    }
                }
                publish_target_mode("ARM");
                std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 每1秒检查一次
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // 等待2秒
            while (rclcpp::ok() && bt_thread_running_)
            {
                {
                    std::lock_guard<std::mutex> lock(mutex_);
                    if (current_mode_ == "TAKEOFF")
                    {
                        RCLCPP_INFO(this->get_logger(), "无人机切换到TAKEOFF模式");
                        break;
                    }
                }
                publish_target_mode("TAKEOFF");
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(15000)); // 等待10秒
            while (rclcpp::ok() && bt_thread_running_)
            {
                {
                    std::lock_guard<std::mutex> lock(mutex_);
                    if (current_mode_ == "GUIDED")
                    {
                        RCLCPP_INFO(this->get_logger(), "无人机切换到GUIDED模式");
                        break;
                    }
                }
                publish_target_mode("GUIDED");
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            }
            return true;
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "起飞异常");
            return false;
        }
    }
    // 主执行函数，执行控制逻辑
    void move_to_gps_target()
    {
        RCLCPP_INFO(this->get_logger(), "move_to_gps_target 已启动");
        // 新增静态变换获取
        while (rclcpp::ok() && bt_thread_running_ && !wait_tf())
        {
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
        if (!take_off_procedure())
            return;

        try
        {
            if (single_gps_nav2_)
            {
                nav2_send_single_goal();
            }
            else
            {
                nav2_send_mult_goal_2();
            }
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "导航调用异常: %s", e.what());
        }
        // 等待导航完成
        rclcpp::WallRate rate(std::chrono::milliseconds(250)); // 设置频率为 1 Hz，即每秒执行一次循环
        while (rclcpp::ok() && bt_thread_running_)
        {
            if (navigation_succeeded_)
            {
                break;
            }
            rate.sleep();
        }
        // 发送启用AprilTag处理器的指令（10Hz持续1秒）
        auto enable_msg = std_msgs::msg::Bool();
        enable_msg.data = true;
        for (int i = 0; i < 10; ++i)
        {
            publisher_enable_apriltag_->publish(enable_msg);
            RCLCPP_INFO(this->get_logger(), "发布 enable_apriltag_processor 为 true");
            std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 10Hz间隔
        }
        while (rclcpp::ok() && bt_thread_running_)
        {
            if (landing_procedure())
            {
                break;
            }
            rate.sleep();
        }
    }

private:
    std::thread move_to_gps_target_thread_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_mode_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_target_mode_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_goal_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_target_cmd_vel_;
    std::string current_mode_;           // 存储当前模式
    std::string apmControllerNameSpace_; // 存储 apmControllerNameSpace（"/apm_drone"）
    std::mutex mutex_;                   // 互斥锁以保证线程安全
    rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr action_client_;
    bool navigation_succeeded_; // 用于标记导航是否成功
    bool use_sim_time_ = false;
    geometry_msgs::msg::TransformStamped base_to_camera_transform_;
    geometry_msgs::msg::TransformStamped base_to_target_transform_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher_enable_apriltag_;
    // 共享数据成员
    std::unordered_map<int, geometry_msgs::msg::TransformStamped> tags_;
    // ROS2相关成员
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    rclcpp::TimerBase::SharedPtr update_timer_;
    rclcpp::TimerBase::SharedPtr display_timer_;

    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr subscription_bt_enable_;
    std::atomic<bool> bt_enabled_;
    std::atomic<bool> bt_thread_running_;
    std::mutex thread_mutex_;

    std::atomic<bool> single_gps_nav2_;

    rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subscription_mult_pose_;
    std::vector<geometry_msgs::msg::PoseStamped> mult_goals_;
    rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SharedPtr action_client_follow_;
};
bool bind_to_cpu(int cpu_core)
{
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu_core, &cpuset);
    pid_t pid = getpid();
    if (sched_setaffinity(pid, sizeof(cpu_set_t), &cpuset) == -1)
    {
        std::cerr << "Failed to bind to CPU " << cpu_core
                  << ": " << strerror(errno) << std::endl;
        return false;
    }
    std::cout << "Process bound to CPU " << cpu_core << std::endl;
    return true;
}
int main(int argc, char **argv)
{
    // 新增参数处理逻辑
    int cpu_core = -1;
    for (int i = 1; i < argc; ++i)
    {
        if (strcmp(argv[i], "--cpu") == 0 && i + 1 < argc)
        {
            cpu_core = atoi(argv[i + 1]);
            // 移除已处理的参数
            for (int j = i; j + 2 <= argc; ++j)
                argv[j] = argv[j + 2];
            argc -= 2;
            break;
        }
    }
    if (cpu_core != -1 && !bind_to_cpu(cpu_core))
    {
        std::cerr << "Continuing without CPU binding" << std::endl;
    }
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<moveToGpsTarget>());
    rclcpp::shutdown();
    return 0;
}
